import serial
import sys
import termios
import tty

class PELCODInterface:
    def __init__(self, port, baudrate):
        self.ser = serial.Serial(port, baudrate)
    
    def send_command(self, data):
        self.ser.write(data)

    def move_left(self, speed=0x20):
        data = bytearray([0xFF, 0x01, 0x00, speed, 0x00, 0x03, (0x20 + speed) % 256])
        self.send_command(data)

    def move_right(self, speed=0x20):
        data = bytearray([0xFF, 0x01, 0x00, 0x00, speed, 0x03, (0x23 + speed) % 256])
        self.send_command(data)

    def set_preset(self, preset_num):
        data = bytearray([0xFF, 0x01, 0x00, 0x00, 0x00, (0x03 + preset_num) % 256, preset_num])
        self.send_command(data)

    def close(self):
        self.ser.close()

# 禁用回显和行缓冲
def disable_echo():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    tty.setraw(sys.stdin.fileno())
    return old_settings

# 恢复回显和行缓冲
def enable_echo(old_settings):
    fd = sys.stdin.fileno()
    termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    


# 主程序
if __name__ == "__main__":\
    
    
    # 初始化串口
    interface = PELCODInterface('/dev/ttyUSB0', 9600)

    # 禁用回显和行缓冲
    old_settings = disable_echo()

    try:
        while True:
            # 读取键盘输入
            key = sys.stdin.read(1)

            # 根据输入控制云台动作
            if key == 'a':
                interface.move_left()
            elif key == 'd':
                interface.move_right()
            elif key == 's':
                preset_num = input("请输入预置点编号：")
                interface.set_preset(int(preset_num))
            elif key == 'q':
                break
            else:
                print("无效的输入")

    finally:
        # 恢复回显和行缓冲
        enable_echo(old_settings)

        # 关闭串口
        interface.close()
